#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

class DepthToPointCloud
{
public:
    DepthToPointCloud():nh_("~"),
        skip_pixel_(1),
        depth_filter_margin_(2),
        depth_filter_mindist_(0.1),
        depth_filter_maxdist_(5.0),
        max_ray_length_(5.0) 
    {
        // 订阅深度图像
        depth_sub_ = nh_.subscribe("/camera/aligned_depth_to_color/image_raw", 1, &DepthToPointCloud::depthCallback, this);
        // 创建 ROS 订阅器，订阅 /camera/pose 话题
        pose_sub_ = nh_.subscribe("/orb_slam3/camera_pose", 10, &DepthToPointCloud::poseCallback, this);

        // 发布点云
        pointcloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/point_cloud", 1);

        // 初始化相机位姿
        camera_pos_ = Eigen::Vector3d(0, 0, 0);
        camera_q_ = Eigen::Quaterniond(1, 0, 0, 0);

        int depth_filter_margin_ = 10;

        // 相机内参（根据具体相机修改）
        fx_ = 429.0;  // 焦距 x
        fy_ = 429.0;  // 焦距 y
        cx_ = 427.5;  // 主点 x
        cy_ = 235.5;  // 主点 y
        depth_scale_ = 1000.0; // 深度缩放比例 (例如 mm 转 m)
    }

    void poseCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
    {
        // 提取位置信息（平移）
        camera_pos_(0) = msg->pose.position.x;
        camera_pos_(1) = msg->pose.position.y;
        camera_pos_(2) = msg->pose.position.z;

        // 提取旋转信息（四元数）
        camera_q_.w() = msg->pose.orientation.w;
        camera_q_.x() = msg->pose.orientation.x;
        camera_q_.y() = msg->pose.orientation.y;
        camera_q_.z() = msg->pose.orientation.z;

        // 输出相机位姿
        // ROS_INFO_STREAM("Camera Position: [" << camera_pos_.transpose() << "]");
        // ROS_INFO_STREAM("Camera Quaternion: [" 
                        // << camera_q_.w() << ", " 
                        // << camera_q_.x() << ", "
                        // << camera_q_.y() << ", " 
                        // << camera_q_.z() << "]");
    }


    void depthCallback(const sensor_msgs::Image::ConstPtr &msg)
    {
        // 转换深度图像
        cv_bridge::CvImageConstPtr cv_ptr;
        try {
            cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_16UC1);
        } catch (cv_bridge::Exception &e) {
            // ROS_ERROR("cv_bridge exception: %s", e.what());
            return;
        }

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

        int rows = cv_ptr->image.rows;
        int cols = cv_ptr->image.cols;
        double inv_depth_scale = 1.0 / depth_scale_;  // 深度缩放因子（例如：1000）

        Eigen::Matrix3d camera_r = camera_q_.toRotationMatrix();

        for (int v = depth_filter_margin_; v < rows - depth_filter_margin_; v += skip_pixel_) {
            const uint16_t *row_ptr = cv_ptr->image.ptr<uint16_t>(v) + depth_filter_margin_;

            for (int u = depth_filter_margin_; u < cols - depth_filter_margin_; u += skip_pixel_) {
                double depth = (*row_ptr++) * inv_depth_scale;

                // 深度范围过滤
                if (depth < depth_filter_mindist_) continue;
                if (depth > depth_filter_maxdist_) depth = max_ray_length_ + 0.1;

                // 投影到三维坐标系
                Eigen::Vector3d pt_cur;
                pt_cur[0] = (u - cx_) * depth / fx_;
                pt_cur[1] = (v - cy_) * depth / fy_;
                pt_cur[2] = depth;

                // 变换到世界坐标系
                Eigen::Vector3d pt_world = camera_r * pt_cur + camera_pos_;

                // 保存点到点云
                pcl::PointXYZ point;
                point.x = pt_world[0];
                point.y = pt_world[1];
                point.z = pt_world[2];
                cloud->points.push_back(point);
            }
        }

        cloud->width = cloud->points.size();
        cloud->height = 1;
        cloud->is_dense = false;

        // 发布点云
        sensor_msgs::PointCloud2 output;
        pcl::toROSMsg(*cloud, output);
        output.header.frame_id = "world";
        pointcloud_pub_.publish(output);

        // 维护上一帧深度图和相机位姿
        last_camera_pos_ = camera_pos_;
        last_camera_q_ = camera_q_;
        last_depth_image_ = cv_ptr->image;
    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber depth_sub_;
    ros::Publisher pointcloud_pub_;
    ros::Subscriber pose_sub_;

    double fx_, fy_, cx_, cy_, depth_scale_; // 相机内参和深度比例
    Eigen::Vector3d camera_pos_;       // 相机的平移向量
    Eigen::Quaterniond camera_q_;      // 相机的旋转四元数

 

    int skip_pixel_;
    int depth_filter_margin_;           // 边缘这个像素以内的会滤除
    double depth_filter_mindist_;
    double depth_filter_maxdist_;
    double max_ray_length_;

    Eigen::Vector3d last_camera_pos_; // 上一帧相机位置
    Eigen::Quaterniond last_camera_q_; // 上一帧相机旋转
    cv::Mat last_depth_image_; // 上一帧深度图像
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "depth_to_pointcloud");
    DepthToPointCloud dtp;
    ros::spin();
    return 0;
}


